//
// GPSMapEdit
// (c) Konstantin Galichsky (kg@geopainting.com), 2002-2004
//
// S42 grid.
//

# include "StdAfx.h"
# include "S42.h"
# include "PtAlgo.h"
# include "fast_floor.h"
# include "Globals.h"

//
// Translate from S42 grid (meters) to latitude/longitude degrees.
// The formula is given in "Geodesy" by I.F.Kushtin (PRIOR, Moscow, 2001), p.138.
//
static
void S42_To_LL (double _x, double _y, point_t & _p) {
	const double b = _y/6367558.497;
	const double fCosB = ::cos (b);
	const double fCosB2 = fCosB*fCosB;
	const double fSinB = ::sin (b);

	const double By = ((2382*fCosB2 + 293609)*fCosB2 + 50221747)*fSinB*fCosB*1e-10 + b;
	const double fCosBy = ::cos (By);
	const double fCosBy2 = fCosBy*fCosBy;
	const double fSinBy = ::sin (By);
	const double fSinBy2 = fSinBy*fSinBy;

	const double A22 = (0.003369263*fCosBy2 +.5)*fSinBy*fCosBy;
	const double A24 = ((0.0056154 - 0.0000151*fCosBy2)*fCosBy2 + 0.1616128)*fCosBy2 + 0.25;
	const double A26 = ((0.00389*fCosBy + 0.04310)*fCosBy2 - 0.00168)*fCosBy2 + 0.125;
	const double A28 = ((0.0013*fCosBy2 + 0.008)*fCosBy2 - 0.031)*fCosBy2 + 0.078;

	const double B13 = (0.16666667 - 0.00112309*fCosBy2)*fCosBy2 - 0.33333333;
	const double B15 = ((0.008783 - 0.000112*fCosBy2)*fCosBy2 - 0.166667)*fCosBy2 + 0.2;
	const double B17 = (0.1667 - 0.0361*fCosBy2)*fCosBy2 - 0.1429;
	const double Ny = ((0.605*fSinBy2 + 107.155)*fSinBy2 + 21346.142)*fSinBy2 + 6378245;

	const double z = _x/(Ny*fCosBy);
	const double z2 = z*z;

	_p.y = 180/c_PI*(By + (((A28*z2 - A26)*z2 + A24)*z2 - 1)*z2*A22);
	_p.x = 180/c_PI*((((B17*z2 + B15)*z2 + B13)*z2 + 1)*z);
}

CS42Grid::CS42Grid (float _fAveX) {
	m_fFalseEasting = _fAveX < 100000 ? 0 : 500000;
	m_iZone = fast_floor ((_fAveX + 500000 - m_fFalseEasting)/1000000);
}

void CS42Grid::ApplyTo (point_t & _point) const {
	_point.x -= 1000000.*m_iZone + m_fFalseEasting;
	S42_To_LL (_point.x, _point.y, _point);
	_point.x += m_iZone*6 - 3;
}
